#include "modules/MonitorModule/TiltMonitor.h"
#include "modules/CtrlModule/PassivePlanner/TiltReactionPlanner.h"
#include "modules/CtrlModule/MarkPosePubModel.h"
#include "lib/WrapFuncs.h"

namespace behavior_controller
{
    TiltMonitor tilt_monitor;
    void TiltMonitor::run()
    {
        collecting_data();
        if (tilt_reaction_planner.finished) //执行倾角响应后不会再进入，直至finished为true
        {
            if (tilt_angle >=MAX_TILT && !DataSet::off_ground_switch.GetData())
            {
                if(anti_shake())
                {
                    DataSet::ctrl_state = TILT_PLAN;
                    tilt_reaction_planner.finished= false;
                    PointF robot_pose_tmp(DataSet::robot_pose.x(),DataSet::robot_pose.y());
                    std::vector<PointF> pose_array;
                    pose_array.emplace_back(robot_pose_tmp);
                    mark_pose_pub_mod.MarkPosePub(pose_array); 
                    if( fabs(DataSet::imu_data.pitch()) > DataSet::imu_data.roll())
                    {
                        if ( DataSet::imu_data.pitch()< 0)
                        {
                            tilt_reaction_planner.linear_vel_sig=-1;
                            tilt_reaction_planner.angular_vel_sig= 1;
                        }
                        else
                        {
                            tilt_reaction_planner.linear_vel_sig= 1;
                            tilt_reaction_planner.angular_vel_sig= -1;
                        }
                        
                        double delt_angle = atan2(DataSet::imu_data.roll(),DataSet::imu_data.pitch());
                        tilt_reaction_planner.global_goal_angle= Funcs::NormalizeAngle(DataSet::robot_pose.angle()-delt_angle-M_PI_2); 
                        //double dst_angle= Funcs::NormalizeAngle(DataSet::robot_pose.angle() - tilt_reaction_planner.global_goal_angle);
                        //std::cout<< "delt_angle= "<< delt_angle<< " global_goal_angle= "<< tilt_reaction_planner.global_goal_angle<< " robot_pose_angle= "<< DataSet::robot_pose.angle()<<std::endl;                  
                        //std::cout<< "dst_angle= "<< dst_angle  << std::endl;
                    }
                    std::cout<< "tilt  bad !"<< std::endl; 
                } 
            }           
        }
        else
        {
            DataSet::ctrl_state = TILT_PLAN;
        }
    }
    void TiltMonitor::collecting_data()
    { 
        if(accum_angle.size()> 100)
            accum_angle.pop_front();
        if(time< 1e-4)
        {
            time= Time::Now();
            tilt_angle= DataSet::imu_data.max_abs_tilt();
            accum_angle.push_back(tilt_angle);
        }
        if(Time::Now()- time> 0.05)
        {
            time=0;
            tilt_angle= DataSet::imu_data.max_abs_tilt();
            accum_angle.push_back(tilt_angle);
        }
    }
    bool TiltMonitor::anti_shake()
    {
        sum_angle = std::accumulate(accum_angle.begin(),accum_angle.end(),0.0);
        return sum_angle>= MID_TILT*100;
    }




} // namespace behavior_controller

